{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import open3d as o3d\n",
    "import time\n",
    "\n",
    "# 读取深度图像\n",
    "depth_img = cv2.imread(\"../img/1_depth.png\", cv2.IMREAD_GRAYSCALE)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 68,
   "metadata": {},
   "outputs": [],
   "source": [
    "h, w = depth_img.shape\n",
    "# 相机内参矩阵\n",
    "k = np.array([[535.4, 0.0, 320.0],\n",
    "              [0.0, 539.2, 240.0],\n",
    "              [0.0, 0.0, 1.0]])\n",
    "\n",
    "# 每次都一样，避免重复计算\n",
    "i, j = np.indices((h, w))\n",
    "static_array = np.stack((i, j, np.ones((h, w))), axis=2).reshape(-1, 3)\n",
    "# 获取相机内参的逆矩阵\n",
    "k_inv = np.linalg.inv(k)\n",
    "point_array = np.dot(k_inv, np.vstack([np.indices((w, h)).reshape(2, -1), np.ones((1,w * h))]))\n",
    "\n",
    "def depth_to_point_cloud_v4(depth_img):\n",
    "    # 使用矩阵乘法计算每个像素点的3D坐标\n",
    "    point = point_array * depth_img.reshape(-1).T\n",
    "    return point"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[  0.,   0.,   0., ..., 479., 479., 479.],\n",
       "       [  0.,   1.,   2., ..., 637., 638., 639.],\n",
       "       [  1.,   1.,   1., ...,   1.,   1.,   1.]])"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "a = np.indices((w, h)).reshape(2, -1)\n",
    "b = np.ones((1,w * h))\n",
    "np.vstack((a, b))\n",
    "\n",
    "# stack是形成新维度，hstack不会形成新维度\n",
    "# np.stack((a, b), axis=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 69,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Time elapsed average 10 times:0.0023 seconds.\n"
     ]
    }
   ],
   "source": [
    "# 十次求平均\n",
    "elapsed_time_list = []\n",
    "for i in range(10):# \n",
    "    start_time = time.time()  # 记录开始时间\n",
    "\n",
    "    # 将深度图转换为点云\n",
    "    pc = depth_to_point_cloud_v4(depth_img)\n",
    "\n",
    "    end_time = time.time()  # 记录结束时间\n",
    "\n",
    "    elapsed_time = end_time - start_time  # 计算耗时\n",
    "\n",
    "    elapsed_time_list.append(elapsed_time)\n",
    "\n",
    "average_time = sum(elapsed_time_list) / len(elapsed_time_list)\n",
    "\n",
    "print(f\"Time elapsed average 10 times:{average_time:.4f} seconds.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 71,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "         5 function calls in 0.003 seconds\n",
      "\n",
      "   Ordered by: standard name\n",
      "\n",
      "   ncalls  tottime  percall  cumtime  percall filename:lineno(function)\n",
      "        1    0.002    0.002    0.002    0.002 26705129.py:14(depth_to_point_cloud_v4)\n",
      "        1    0.000    0.000    0.003    0.003 <string>:1(<module>)\n",
      "        1    0.000    0.000    0.003    0.003 {built-in method builtins.exec}\n",
      "        1    0.000    0.000    0.000    0.000 {method 'disable' of '_lsprof.Profiler' objects}\n",
      "        1    0.000    0.000    0.000    0.000 {method 'reshape' of 'numpy.ndarray' objects}\n",
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "import cProfile\n",
    "cProfile.run('depth_to_point_cloud_v4(depth_img)')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 67,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 可视化点云\n",
    "\n",
    "# 点云数据存储在名为point_cloud的numpy数组中\n",
    "cloud = o3d.geometry.PointCloud()\n",
    "cloud.points = o3d.utility.Vector3dVector(pc)\n",
    "\n",
    "# 创建一个可视化窗口\n",
    "vis = o3d.visualization.Visualizer()\n",
    "vis.create_window()\n",
    "\n",
    "# 将点云添加到可视化窗口中\n",
    "vis.add_geometry(cloud)\n",
    "\n",
    "# 设置相机位置，没调所以可有可无\n",
    "vis.get_view_control().set_front([0, 0, -1])\n",
    "vis.get_view_control().set_lookat([0, 0, 0])\n",
    "vis.get_view_control().set_up([0, -1, 0])\n",
    "\n",
    "# 开始显示点云\n",
    "vis.run()\n",
    "\n",
    "# 关闭显示窗口\n",
    "vis.destroy_window()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "0.0027s! 简化了代码"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "env4cv",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
